﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  class MBLMultiStartPointGradientSelfLocator : SelfLocator
  {
    private int[] randomNumbers = new int[10];
    private int numberOfStatesInPositionConstraint = 0;

    public override WorldState Execute(Experiment experiment)
    {
      return DoVeryIntelligentGradientDescent(experiment);
    }
    public WorldState DoVeryIntelligentGradientDescent(Experiment experiment)
    {
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);
      if (listOfOldPlaces.Count == 0) return null;

      Vector2 bottomLeftCorner = new Vector2(Globals.stateSpaceMinimumX, Globals.stateSpaceMinimumY);
      Vector2 topRightCorner = new Vector2(Globals.stateSpaceMaximumX, Globals.stateSpaceMaximumY);
      int rasterWidth = 5;
      int xCounter = 0;
      int yCounter = 0;
      List<WorldState2D> lastMeasurementPositionConstraint = new List<WorldState2D>();

      for (int x = (int)bottomLeftCorner.x; x <= (int)topRightCorner.x; x += rasterWidth)
      {
        yCounter = 0;
        for (int y = (int)bottomLeftCorner.y; y <= (int)topRightCorner.y; y += rasterWidth)
        {
          WorldState2D worldState = new WorldState2D(x, y);
          worldState.Move((WorldState2D)listOfOldPlaces[0].worldState);

          int index = 0;
          while(index < experiment.sensors.Count)
          {
            Measurement prediction = listOfOldPlaces[index].measurement.sensor.PredictPerfect(worldState, experiment.map);
            double difference = prediction.GetDifference(listOfOldPlaces[index].measurement);

            if (Math.Pow(difference, 2) < 10)
            {
              lastMeasurementPositionConstraint.Add(new WorldState2D(x, y));
              DebugDrawingTool.CreateCircle("world:MBLMultiGradient:last measurement constraint", new Vector2(x, y), Globals.rasterWidth / 5, 4, Color.Red);
            }
            index++;
          }
          yCounter++;
        }//for y..
        xCounter++;
      }//for x..

      if (lastMeasurementPositionConstraint.Count == 0) return null;

      Random rnd = new Random();

      bool createNewNumbers = true;
      if (DebugKeyTool.IsActive("MBLMultiGradient:freeze gd starting points"))
      {
        if (lastMeasurementPositionConstraint.Count == numberOfStatesInPositionConstraint)
          createNewNumbers = false;
      }
      if (createNewNumbers)
      {
        for (int i = 0; i < 10; i++)
        {
          randomNumbers[i] = rnd.Next(lastMeasurementPositionConstraint.Count - 1);
        }
      }
      
      WorldState2D bestState = null;
      double minimum = Double.MaxValue;

      for (int i = 0; i < 10; i++)
      {
        WorldState2D selectedState = lastMeasurementPositionConstraint[randomNumbers[i]];
        WorldState2D resultState;
        DebugDrawingTool.CreateCircle("world:MBLMultiGradient:selection", new Vector2(selectedState.Vector.x, selectedState.Vector.y), Globals.rasterWidth / 3, 4, Color.Orange);
        resultState = MBLTools.DoGradientDescent(new Vector2(selectedState.Vector.x, selectedState.Vector.y), experiment);
        if (DebugKeyTool.IsActive("MBLMultiGradient:intelligent version"))
          resultState = MBLTools.DoIntelligentGradientDescent(new Vector2(selectedState.Vector.x, selectedState.Vector.y), experiment);

        double valueOfFunctionAtResultPosition = MBLTools.CalculateFunction(listOfOldPlaces, resultState.Vector.x, resultState.Vector.y, experiment);
        if (valueOfFunctionAtResultPosition < minimum)
        {
          minimum = valueOfFunctionAtResultPosition;
          bestState = resultState;
        }
      }
      numberOfStatesInPositionConstraint = lastMeasurementPositionConstraint.Count;
      return bestState;
    }//method
  }//class
}
